﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using PloobsEngine.SceneControl;
using PloobsEngine.Physics;
using PloobsEngine.Draw;

namespace Etapa1.SteeringExperiments
{
    public class Avoidance : Steering
    {        

        public Avoidance(IWorld world)
            : base(BehaviorType.avoidance)
        {
            this.world = world;
        }
        private IWorld world;

        private float sensorSize = 3;
        private float smallerFellerSizeProporcional = 1.0f / 3;

        public float SmallerFellerSizeProporcional
        {
            get { return smallerFellerSizeProporcional; }
            set { smallerFellerSizeProporcional = value; }
        }

        public float SensorSize
        {
            get { return sensorSize; }
            set { sensorSize = value; }
        }
        private float angleBetweenSensors = 30;

        public float AngleBetweenSensors
        {
            get { return angleBetweenSensors; }
            set { angleBetweenSensors = value; }
        }

        private float distanceFromCenter = 2;

        public float DistanceFromCenter
        {
            get { return distanceFromCenter; }
            set { distanceFromCenter = value; }
        }

        public override Microsoft.Xna.Framework.Vector3 CalculateSteering(SteeringEntityInfo entity)
        {
            Vector3 vel = entity.Obj.PhysicObject.Velocity;
            float vlenght = vel.Length();
            if(vel!=Vector3.Zero)
            vel.Normalize();

            float sRealSize = sensorSize/2 * vlenght + sensorSize;

            Ray raio = new Ray(entity.Obj.Position + vel * distanceFromCenter, vel * sRealSize + vel * distanceFromCenter);
            Drawing.Draw3dLine(raio.Position, raio.Direction + entity.Obj.Position, Color.Green);
            RayTestInfo ri = world.SegmentIntersect(raio, SegmentInterceptMethod.ALL, sRealSize);
                        
            float sz = sRealSize * smallerFellerSizeProporcional;
            Vector3 dir4 = Vector3.Transform(vel, Matrix.CreateRotationY(MathHelper.ToRadians(-angleBetweenSensors)));
            Ray raio4 = new Ray(entity.Obj.Position + dir4 * distanceFromCenter, dir4 * sz + dir4 * distanceFromCenter);
            Drawing.Draw3dLine(raio4.Position, raio4.Direction + entity.Obj.Position, Color.White);
            RayTestInfo ri2 = world.SegmentIntersect(raio4, SegmentInterceptMethod.ALL, sz);

            Vector3 dir5 = Vector3.Transform(vel, Matrix.CreateRotationY(MathHelper.ToRadians(angleBetweenSensors)));
            Ray raio5 = new Ray(entity.Obj.Position + dir5 * distanceFromCenter, dir5 * sz + dir5 * distanceFromCenter);
            Drawing.Draw3dLine(raio5.Position, raio5.Direction + entity.Obj.Position, Color.White);
            RayTestInfo ri3 = world.SegmentIntersect(raio5, SegmentInterceptMethod.ALL, sz);

            Vector3 ret = Vector3.Zero;

            if (ri != null && ri.Object != entity.Obj)
            {
                    float dReal = sRealSize - ri.Distance;
                    dReal = (float)Math.Exp(dReal);
                    Drawing.Draw3dLine(ri.ImpactPosition, ri.ImpactPosition + ri.ImpactNormal * 10, Color.Brown);
                    ret += (ri.ImpactNormal * dReal/2 );                
            }


            Vector3 v2 = Vector3.Zero;
            Vector3 v3 = Vector3.Zero;

            if (ri2 != null && ri2.Object != entity.Obj)
            {
                float dReal = sz- ri2.Distance;
                dReal = (float)Math.Exp(dReal);
                //Drawing.DrawLine(ri2.ImpactPosition, ri2.ImpactPosition + ri2.ImpactNormal * 10, Color.Brown);
                //v2 = (ri2.ImpactNormal * dReal);                
                Vector3 dirx = Vector3.Transform(vel, Matrix.CreateRotationY(MathHelper.ToRadians(90)));
                if(dirx!=Vector3.Zero)
                dirx.Normalize();
                Drawing.Draw3dLine(ri2.ImpactPosition, ri2.ImpactPosition + dirx * 10, Color.Brown);
                v2 = (dirx * dReal);

            }

            if (ri3 != null && ri3.Object != entity.Obj)
            {
                float dReal = sz - ri3.Distance;
                dReal = (float) Math.Exp(dReal);
                //Drawing.DrawLine(ri3.ImpactPosition, ri3.ImpactPosition + ri3.ImpactNormal * 10, Color.Brown);
                //v3 = (ri3.ImpactNormal * dReal);                
                Vector3 dirx = Vector3.Transform(vel, Matrix.CreateRotationY(MathHelper.ToRadians(-90)));
                if (dirx != Vector3.Zero)
                dirx.Normalize();
                Drawing.Draw3dLine(ri3.ImpactPosition, ri3.ImpactPosition + dirx * 10, Color.Brown);
                v3 = (dirx * dReal);
            }


            if (v2 == Vector3.Zero && v3 == Vector3.Zero)
            {
                return ret;
            }
            else if (v2 == Vector3.Zero)
            {
                return ret + v3;
            }
            else if (v3 == Vector3.Zero)
            {
                return ret + v2;
            }
            else if (v2.Length() > v3.Length())
            {
                return ret + v2;
            }
            else
            {
                return ret + v3;
            }            
            
        }
    }
}
